
//#include <chrono>
// #include <atomic>

#include "ddsMain.hpp"
#include <memory>

#define METHOD_PROLOGUE(theClass, localClass) \
theClass* pThis = ((theClass*)((BYTE*)this - m_nOffset)); \
AFX_MANAGE_STATE(pThis->m_pModuleState) \
pThis;

using namespace std;

// ProtocolSuite& ddsProtocol::operator=(const ProtocolSuite& pSuite)
// {

// }


ddsProtocol::ddsProtocol(Resolver<ProtocolSuite> *handle)
        : publisher_(nullptr)
        , pubTopic_(nullptr)
        , writer_(nullptr)
        , pubParticipant_(nullptr)
        , pubType_(new ProtocolSuitePubSubType())

        , subParticipant_(nullptr)
        , subscriber_(nullptr)
        , reader_(nullptr)
        , subTopic_(nullptr)
        , subType_ (new ProtocolSuitePubSubType())
    {

        subListener_.resolver = handle; //载入数据解析
    };

ddsProtocol::~ddsProtocol()
{
    if (writer_ != nullptr)
    {
        publisher_->delete_datawriter(writer_);
    }
    if (publisher_ != nullptr)
    {
        pubParticipant_->delete_publisher(publisher_);
    }
    if (pubTopic_ != nullptr)
    {
        pubParticipant_->delete_topic(pubTopic_);
    }

    if (reader_ != nullptr)
    {
        subscriber_->delete_datareader(reader_);
    }
    if (subTopic_ != nullptr)
    {
        subParticipant_->delete_topic(subTopic_);
    }
    if (subscriber_ != nullptr)
    {
        subParticipant_->delete_subscriber(subscriber_);
    }
    DomainParticipantFactory::get_instance()->delete_participant(pubParticipant_);
    DomainParticipantFactory::get_instance()->delete_participant(subParticipant_);
    
}



bool ddsProtocol::init(int part, const char * pub_topicName, const char * sub_topicName)
{
    if((pub_topicName == nullptr) || (sub_topicName == nullptr))
    {
        return false;
    }

    pubListener_.m_pubName = pub_topicName;
    subListener_.m_subName = sub_topicName;
    /*pub init*/
    DomainParticipantQos participantQos;
    participantQos.name("Participant_publisher");
    pubParticipant_ = DomainParticipantFactory::get_instance()->create_participant(part, participantQos);

    if (pubParticipant_ == nullptr)
    {
        return false;
    }

    //REGISTER THE TYPE
    pubType_.register_type(pubParticipant_);

    /*sub init*/
    DomainParticipantQos subParticipantQos;
    subParticipantQos.name("Participant_subscriber");
    subParticipant_ = DomainParticipantFactory::get_instance()->create_participant(part, subParticipantQos);

    if (subParticipant_ == nullptr)
    {
        return false;
    }

    // Register the Type
    subType_.register_type(subParticipant_);


/*publisher init */
    qlog_i(  "%s,publisher init !",pub_topicName);
    //CREATE THE TOPIC
    pubTopic_ = pubParticipant_->create_topic(
        pub_topicName,
        pubType_.get_type_name(),
        TOPIC_QOS_DEFAULT);
    if (pubTopic_ == nullptr)
    {
        return false;
    }
    //CREATE THE PUBLISHER
    publisher_ = pubParticipant_->create_publisher(PUBLISHER_QOS_DEFAULT, nullptr);

    if (publisher_ == nullptr)
    {
        return false;
    }
    // CREATE THE WRITER
    DataWriterQos wqos = DATAWRITER_QOS_DEFAULT;
    wqos.reliability().kind = RELIABLE_RELIABILITY_QOS;
    wqos.publish_mode().kind = ASYNCHRONOUS_PUBLISH_MODE;
    wqos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::MemoryManagementPolicy::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
    writer_ = publisher_->create_datawriter(pubTopic_, wqos, &pubListener_);
    if (writer_ == nullptr)
    {
        return false;
    }


/*subscriber init */
    qlog_i("%s,subscriber init!",sub_topicName);
    //CREATE THE TOPIC
    subTopic_ = subParticipant_->create_topic(
        sub_topicName,
        subType_.get_type_name(),
        TOPIC_QOS_DEFAULT);
    if (subTopic_ == nullptr)
    {
        return false;
    }

    // Create the Subscriber
    subscriber_ = subParticipant_->create_subscriber(SUBSCRIBER_QOS_DEFAULT, nullptr);

    if (subscriber_ == nullptr)
    {
        return false;
    }

  //CREATE THE READER
    DataReaderQos rqos = DATAREADER_QOS_DEFAULT;
    rqos.reliability().kind = RELIABLE_RELIABILITY_QOS;
    rqos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::MemoryManagementPolicy::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
    // Create the DataReader
    reader_ = subscriber_->create_datareader(subTopic_, rqos, &subListener_);

    if (reader_ == nullptr)
    {
        return false;
    }

    qlog_i("dds init success!");
    
    return true;
}

//Protocol to send
int ddsProtocol::publish(struct cmdDataType* cmd,uint8_t * cmdData)
{
    if (pubListener_.matched_ > 0)
    {
        ProtocolSuite Protocolst;
        if(cmd->dataLen > 0){
            std::vector<uint8_t> nSendData(cmdData,(cmdData+cmd->dataLen));
            Protocolst.Message(std::move(nSendData));
        }
        
        Protocolst.Index(Protocolst.Index() + 1);
        
        Protocolst.DevType(cmd->DeviceType);
        Protocolst.SrcDevNum(cmd->source_DevNum);
        Protocolst.SrcDevType(cmd->source_DevType);
        Protocolst.DstDevType(cmd->dst_DevType);
        Protocolst.DstDevNum(cmd->dst_DevNum);
        Protocolst.CmdType(cmd->cmdType);
        Protocolst.CmdChildType(cmd->cmdChildType);
        Protocolst.devState(cmd->DevStatus);
        Protocolst.DataLen(cmd->dataLen);
 
        writer_->write(&Protocolst);
        return 0;
    }
    return -1;
}

int ddsProtocol::publish(ProtocolSuite& suite)
{
    if (pubListener_.matched_ > 0)
    {
        writer_->write(&suite);
        return 0;
    }
    return -1;
}


/*配对函数，dds配对发生变化后会自动调用此函数*/
void ddsProtocol::PubListener::on_publication_matched(DataWriter* ,const PublicationMatchedStatus& info)
{
    //METHOD_PROLOGUE(DDS_Port,PubListener);

     if (info.current_count_change == 1)
    {   
        matched_ = info.total_count;
        qlog_w("%s,Publisher matched.",m_pubName.c_str());
    }
    else if (info.current_count_change == -1)
    {
        matched_ = info.total_count;
        qlog_w("%s,Publisher unmatched.",m_pubName.c_str());
    }
    else
    {
        std::cout << info.current_count_change
            << " is not a valid value for PublicationMatchedStatus current count change." << std::endl;
    }
}

void ddsProtocol::SubListener::on_subscription_matched(
        DataReader*,
        const SubscriptionMatchedStatus& info)
{
    if (info.current_count_change == 1)
    {
        qlog_w("%s,Subscriber matched.",m_subName.c_str());
        // std::cout << m_subName << "Subscriber matched"<< std::endl;
    }
    else if (info.current_count_change == -1)
    {
        qlog_w("%s,Subscriber unmatched.",m_subName.c_str());
        // std::cout << m_subName << "Subscriber unmatched"<< std::endl;
    }
    else
    {
        std::cout << info.current_count_change
                << " is not a valid value for SubscriptionMatchedStatus current count change" << std::endl;
    }
}

void ddsProtocol::SubListener::on_data_available(
        DataReader* reader)
{
    std::unique_ptr<ProtocolSuite> st = std::make_unique<ProtocolSuite>();
    // ProtocolSuite *st = new ProtocolSuite();
    SampleInfo info;
    while (reader->take_next_sample(st.get(), &info) == ReturnCode_t::RETCODE_OK)
    {
        if (info.valid_data)
        {
            samples_++;
            
            if (resolver != nullptr) {
                resolver->dataPush(st.get());
            }  
        }
    }
}